#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <cmath>
#include <vector>
#include <mutex>

class ClipLocalMap
{
public:
    ClipLocalMap() : nh_("~"), tf_buffer_(), tf_listener_(tf_buffer_)
    {
        // 订阅全局地图
        map_sub_ = nh_.subscribe("/map", 10, &ClipLocalMap::mapCallback, this);

        // 发布局部地图
        local_map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/local_map_clip", 10);

        // 定时器设置 (0.1秒间隔)
        timer_ = nh_.createTimer(ros::Duration(0.1), &ClipLocalMap::timerCallback, this);
    }

private:
    void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
    {
        std::lock_guard<std::mutex> lock(map_mutex_);
        global_map_ = *msg;
    }

    void timerCallback(const ros::TimerEvent &)
    {
        std::lock_guard<std::mutex> lock(map_mutex_);
        if (!global_map_.header.stamp.isZero())
        {
            geometry_msgs::TransformStamped trans;
            try
            {
                // 获取最新的可用变换，而非当前时间点的变换
                trans = tf_buffer_.lookupTransform("map", "body", ros::Time(0), ros::Duration(0.1));
            }
            catch (tf2::TransformException &ex)
            {
                ROS_WARN_STREAM("TF transform unavailable: " << ex.what());
                return;
            }

            // 提取位置和姿态
            double x = trans.transform.translation.x;
            double y = trans.transform.translation.y;
            tf2::Quaternion q(
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w);
            tf2::Matrix3x3 m(q);
            double roll, pitch, yaw;
            m.getRPY(roll, pitch, yaw);

            // 使用变换的时间戳
            publishLocalMap(x, y, yaw, trans.header.stamp);
        }
    }

    void publishLocalMap(double x, double y, double yaw, ros::Time stamp)
    {
        const auto &info = global_map_.info;
        float resolution = info.resolution;
        int map_width = info.width;
        int map_height = info.height;
        double origin_x = info.origin.position.x;
        double origin_y = info.origin.position.y;

        // 计算局部地图尺寸
        int width = static_cast<int>(12.0 / resolution);
        int height = static_cast<int>(12.0 / resolution);

        // 构建局部地图
        nav_msgs::OccupancyGrid local_map;
        local_map.header.stamp = stamp;
        local_map.header.frame_id = "body";
        local_map.info = info;
        local_map.info.width = width;
        local_map.info.height = height;
        local_map.info.origin.position.x = -6.0;
        local_map.info.origin.position.y = -6.0;
        local_map.info.origin.orientation.w = 1.0;
        local_map.data.resize(width * height);

        // 填充数据
        for (int row = 0; row < height; ++row)
        {
            for (int col = 0; col < width; ++col)
            {
                double dx = (col + 0.5) * resolution - 6.0;
                double dy = (row + 0.5) * resolution - 6.0;

                // 坐标变换
                double x_map = x + dx * cos(yaw) - dy * sin(yaw);
                double y_map = y + dx * sin(yaw) + dy * cos(yaw);

                // 计算索引
                int cell_x = static_cast<int>((x_map - origin_x) / resolution);
                int cell_y = static_cast<int>((y_map - origin_y) / resolution);

                if (cell_x >= 0 && cell_x < map_width &&
                    cell_y >= 0 && cell_y < map_height)
                {
                    int idx = cell_y * map_width + cell_x;
                    local_map.data[row * width + col] = global_map_.data[idx];
                }
                else
                {
                    local_map.data[row * width + col] = -1;
                }
            }
        }

        local_map_pub_.publish(local_map);
    }

    ros::NodeHandle nh_;
    ros::Subscriber map_sub_;
    ros::Publisher local_map_pub_;
    ros::Timer timer_;
    nav_msgs::OccupancyGrid global_map_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;
    std::mutex map_mutex_;
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "clip_local_map_node");
    ClipLocalMap clipper;
    ros::spin();
    return 0;
}